/* 初始化
 PID_Init_Config_s xxx_config = {
     .MaxOut = 2000.0f,
     .IntegralLimit = 1000.0f,
     .Kp = 1000.0f,
     .Ki = 20.0f,
	 .Kd = 0.1f,
	 .Derivative_LPF_RC = 0.008f,
     .Improve = PID_Integral_Limit | PID_DERIVATIVE_FILTER};
 PIDInit(&xxx_pid, &xxx_config); */

#include "pid.h"
#include "string.h"
#include "math.h"

void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
{
    // config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
    memset(pid, 0, sizeof(PIDInstance));
    // 利用其内存连续的结构体的特性, 将剩余内存设置为0
    memcpy(pid, config, sizeof(PID_Init_Config_s));
}

//单级PID计算，需要传入的参数，PID结构体，目标值，反馈值
void PIDCalculate(PIDInstance *pid, float set, float measure)
{
    //更新误差
    pid->Last_Err = pid->Err;
    pid->Err = set - measure;

    //P环节
    pid->Pout = pid->Kp * pid->Err;

    //I环节
    pid->ITerm += pid->Ki * pid->Err;
    LIMIT(pid->ITerm, -pid->IntegralLimit, pid->IntegralLimit);

    //D环节
    pid->Dout = pid->Kd * (pid->Err - pid->Last_Err);

    //总输出
    pid->Output = pid->Pout + pid->ITerm + pid->Dout;
    LIMIT(pid->Output, -pid->MaxOut, pid->MaxOut);
}

// 积分限幅

/// @brief 
/// @param pid 
static void f_Integral_Limit(PIDInstance *pid)
{
    static float temp_Output, temp_Iout;
    temp_Iout = pid->Iout + pid->ITerm;
    temp_Output = pid->Pout + pid->Iout + pid->Dout;
    if (fabsf(temp_Output) > pid->MaxOut)
    {
        if (pid->Err * pid->Iout > 0) // 积分还在累积
        {
            pid->ITerm = 0; // 当前积分项置零
        }
    }

    if (temp_Iout > pid->IntegralLimit)
    {
        pid->ITerm = 0;
        pid->Iout = pid->IntegralLimit;
    }
    if (temp_Iout < -pid->IntegralLimit)
    {
        pid->ITerm = 0;
        pid->Iout = -pid->IntegralLimit;
    }
}
